
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       math_def.h
  * @author     baiyang
  * @date       2021-7-5
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <math.h>
#include <stdint.h>
#include <stdbool.h>
#include <float.h>
/*-----------------------------------macro------------------------------------*/
#ifndef MIN
#define MIN(a, b) (a < b ? a : b)
#endif

#ifndef MAX
#define MAX(a, b) (a > b ? a : b)
#endif

#ifdef M_PI
# undef M_PI
#endif
#define M_PI      (3.141592653589793f)

#ifdef M_PI_2
# undef M_PI_2
#endif
#define M_PI_2    (M_PI / 2)

#define M_GOLDEN  1.6180339f

#ifdef M_2PI
# undef M_2PI
#endif
#define M_2PI         (M_PI * 2)

//弧度转角度
#ifndef Rad2Deg
#define Rad2Deg(x)    ((x) * 57.2957795f)
#endif

//角度转弧度
#ifndef Deg2Rad
#define Deg2Rad(x)    ((x) * 0.0174532925f)
#endif

#ifndef SQ
#define SQ(val)       (val * val)
#endif

#ifndef sq
#define sq(val)       (val * val)
#endif

// MATH_CHECK_INDEXES modifies some objects (e.g. SoloGimbalEKF) to
// include more debug information.  It is also used by some functions
// to add extra code for debugging purposes. If you wish to activate
// this, do it here or as part of the top-level Makefile -
#ifndef MATH_CHECK_INDEXES
  #define MATH_CHECK_INDEXES 0
#endif

#define DEG_TO_RAD      (M_PI / 180.0f)
#define RAD_TO_DEG      (180.0f / M_PI)

// Centi-degrees to radians
#define DEGX100 5729.57795f

// degrees -> radians
#ifndef radians
#define radians(x)    ((x) * DEG_TO_RAD)
#endif

// radians -> degrees
#ifndef degrees
#define degrees(x)    ((x) * RAD_TO_DEG)
#endif

#ifndef ToRad
#define ToRad(x) radians(x)	// *pi/180
#endif

#ifndef ToDeg
#define ToDeg(x) degrees(x)	// *180/pi
#endif

// GPS Specific double precision conversions
// The precision here does matter when using the wsg* functions for converting
// between LLH and ECEF coordinates.
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
#define  DEG_TO_RAD_DOUBLE  (asin(1) / 90)
#define  RAD_TO_DEG_DOUBLE  (1 / DEG_TO_RAD_DOUBLE)
#endif

#define RPM_TO_RPS        (M_2PI / 60)
#define RPS_TO_RPM        (60 / M_2PI)

#define SSL_AIR_DENSITY         1.225f // kg/m^3
#define SSL_AIR_PRESSURE       101325.01576f // Pascal
#define SSL_AIR_TEMPERATURE    288.15f // K

#define RadiansToCentiDegrees(x) ((float)(x) * RAD_TO_DEG * 100.0f)

// acceleration due to gravity in m/s/s
#define GRAVITY_MSS     9.80665f

// radius of earth in meters
#define RADIUS_OF_EARTH 6378100

// convert a longitude or latitude point to meters or centimeters.
// Note: this does not include the longitude scaling which is dependent upon location
#define LATLON_TO_M     0.011131884502145034
#define LATLON_TO_M_INV 89.83204953368922
#define LATLON_TO_CM    1.1131884502145034

// scaling factor from 1e-7 degrees to meters at equator
// == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
#define LOCATION_SCALING_FACTOR  0.011131884502145034f

// inverse of LOCATION_SCALING_FACTOR
#define LOCATION_SCALING_FACTOR_INV  89.83204953368922f

// Semi-major axis of the Earth, in meters.
#define WGS84_A  6378137.0

//Inverse flattening of the Earth
#define WGS84_IF  298.257223563

// The flattening of the Earth
#define WGS84_F  ((double)1.0 / WGS84_IF)

// Semi-minor axis of the Earth in meters
#define WGS84_B  (WGS84_A * (1 - WGS84_F))

// Eccentricity of the Earth
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
#define WGS84_E (sqrt(2 * WGS84_F - WGS84_F * WGS84_F))
#endif

#define C_TO_KELVIN 273.15f

#define M_PER_SEC_TO_KNOTS 1.94384449f
#define KNOTS_TO_M_PER_SEC (1/M_PER_SEC_TO_KNOTS)

#define KM_PER_HOUR_TO_M_PER_SEC 0.27777778f

// Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
#define ISA_GAS_CONSTANT 287.26f
#define ISA_LAPSE_RATE 0.0065f

// Standard Sea Level values
// Ref: https://en.wikipedia.org/wiki/Standard_sea_level
#define SSL_AIR_DENSITY         1.225f // kg/m^3
#define SSL_AIR_PRESSURE 101325.01576f // Pascal
#define SSL_AIR_TEMPERATURE    288.15f // K

#define INCH_OF_H2O_TO_PASCAL 248.84f

/*
  use AP_ prefix to prevent conflict with OS headers, such as NuttX
  clock.h
 */
#define GP_NSEC_PER_SEC   1000000000ULL
#define GP_NSEC_PER_USEC  1000ULL
#define GP_USEC_PER_SEC   1000000ULL
#define GP_USEC_PER_MSEC  1000ULL
#define GP_MSEC_PER_SEC   1000ULL
#define GP_SEC_PER_WEEK   (7ULL * 86400ULL)
#define GP_MSEC_PER_WEEK  (AP_SEC_PER_WEEK * AP_MSEC_PER_SEC)

// speed and distance conversions
#define KNOTS_TO_METERS_PER_SECOND 0.51444
#define FEET_TO_METERS 0.3048

// Convert amps milliseconds to milliamp hours
// Amp.millisec to milliAmp.hour = 1/1E3(ms->s) * 1/3600(s->hr) * 1000(A->mA)
#define AMS_TO_MAH 0.000277777778f

#ifndef LOWBYTE
#define LOWBYTE(i) ((uint8_t)(i))
#endif

#ifndef HIGHBYTE
#define HIGHBYTE(i) ((uint8_t)(((uint16_t)(i))>>8))
#endif

#ifndef ARRAY_SIZE
#define ARRAY_SIZE(_arr) (sizeof(_arr)/sizeof(_arr[0]))
#endif
/*----------------------------------typedef-----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



